/*
* KINOVA (R) KORTEX (TM)
*
* Copyright (c) 2018 Kinova inc. All rights reserved.
*
* This software may be modified and distributed
* under the terms of the BSD 3-Clause license.
*
* Refer to the LICENSE file for details.
*
*/

#include <Base.pb.h>

#include <google/protobuf/util/json_util.h>

namespace k_api = Kinova::Api;

#define IP_ADDRESS "192.168.1.10"
#define PORT 10000

void example_manipulation_protobuf_basic()
{
    // In Google Protocol Buffer, there are many scalar value types you can declare. 
    // All these types have a corresponding type in C++. 
    // Here's the list:

    // Protobuf type : C++ type
    // double : double
    // float : float
    // int32 : int32
    // int64 : int64
    // uint32 : uint32
    // uint64 : uint64
    // sint32 : int32
    // sint64 : int64
    // fixed32 : uint32 
    // fixed64 : uint64
    // sfixed32 : int32
    // sfixed64 : int64
    // bool : bool
    // string : string
    // bytes : string

    // These types must only be accessed using the setter generated by the API.
    // For more information about Scalar Value Types refer to:
    // https://developers.google.com/protocol-buffers/docs/proto3#scalar

    // You can regroup many of these scalar values in a message. The message is a structure used in Google Protocol Buffer
    // to ensure all information is within the scope of the object. If they are not contained in a message, 
    // scalar values can't exist on their own.

    // Here's a quick example using the Kinova API UserProfile message:
    // message UserProfile {
    //     Kinova.Api.Common.UserProfileHandle handle = 1; // User handle (no need to set it with CreateUserProfile()
    //     string username = 2;                            // username used to connect to robot (or via Web App login)
    //     string firstname = 3;                           // user first name
    //     string lastname = 4;                            // user last name
    //     string application_data = 5;                    // other application data (used by Web App)
    // }
    

    auto user_profile = k_api::Base::UserProfile();
    // Each scalar value in a message has a set_<field> function to set the value and a getter which is
    // simply the variable name
    user_profile.set_username("jcash");
    user_profile.set_firstname("Johnny");
    user_profile.set_lastname("Cash");
    // Handle and application_data are ignored on purpose
}


void example_manipulation_protobuf_object()
{
    // Messages are the main elements in Google Protocol Buffer in the same way classes are to C++. You need a message
    // to make a workable object. A message can contain many kinds of elements. We have already 
    // covered the scalar value, and in this section we are going to cover the message.

    // A message can make a reference to another message to make a more comprehensive element.

    // For this example we'll use the FullUserProfile and UserProfile messages.
    // message FullUserProfile { 
    //     UserProfile user_profile = 1; // User Profile, which includes username
    //     string password = 2;          // User password
    // }
    // message UserProfile {
    //     Kinova.Api.Common.UserProfileHandle handle = 1; // User handle (no need to set it with CreateUserProfile()
    //     string username = 2;                            // username, used to connect to robot (or via Web App login)
    //     string firstname = 3;                           // user first name
    //     string lastname = 4;                            // user last name
    //     string application_data = 5;                    // other application data (used by Web App)
    // }

    // https://developers.google.com/protocol-buffers/docs/proto3#simple

    auto full_user_profile = k_api::Base::FullUserProfile();
    // Now add data to the scalar
    full_user_profile.set_password("MyPassword");

    // Each message will have the function to get a mutable from the underlying message.
    auto user_profile = full_user_profile.mutable_user_profile();
    user_profile->set_username("jcash");
    user_profile->set_firstname("Johnny");
    user_profile->set_lastname("Cash");

    // Another basic element is the enum. Enum are directly available from the message - no need to use the enum 'message'.
    // Here's an example:
    // enum LimitationType {
    //     UNSPECIFIED_LIMITATION = 0;  // unspecified limitation
    //     FORCE_LIMITATION = 1;        // force limitation
    //     ACCELERATION_LIMITATION = 2; // acceleration limitation
    //     VELOCITY_LIMITATION = 3;     // velocity limitation
    // }

    // message LimitationTypeIdentifier { 
    //     LimitationType type = 1;     // limitation type
    // }

    // https://developers.google.com/protocol-buffers/docs/proto3#enum

    auto limitation_type = k_api::Base::CartesianLimitation();
    limitation_type.set_type(k_api::Base::FORCE_LIMITATION);
}


void example_manipulation_protobuf_list()
{
    // In Google Protocol Buffer, 'repeated' is used to designate a list of indeterminate length (there is no equivalent in C++).
    // These are handled using the provided method associated with repeated object.

    // The message using 'repeated' has some embedded functions generated by Google Protocol Buffer.

    // In this example we will use the following messages
    
   // An array of joint angles values with specified constraint
   // message ConstrainedJointAngles {
   //		JointAngles joint_angles = 1;               // Joint angles values
   //		JointTrajectoryConstraint constraint = 2;   // Constraint to apply to all the joint angles (optional)
   // }

   // An array of joint angles
   // message JointAngles {
   //		repeated JointAngle joint_angles = 1; // Array of joint angles
   // }
  
   // Angle value of a specific joint
   // message JointAngle {
   //   	uint32 joint_identifier = 1;    // Joint identifier
   //       float value = 2;                // Position (in degrees)
   // }

    // To keep the example clearer the attribute action in SequenceTask message will be kept to default value

    // For more details please visit:
    // https://developers.google.com/protocol-buffers/docs/reference/cpp-generated#repeatedmessage

    k_api::Base::ConstrainedJointAngles constrained_joint_angles = k_api::Base::ConstrainedJointAngles();
    auto joint_angles = constrained_joint_angles.mutable_joint_angles(); // Returns a pointer to the joint_angles message

    for (auto i = 0; i > 7; i++)
    {
        auto joint_angle = joint_angles->add_joint_angles(); // This will create and return a repeated element pointer
        joint_angle->set_joint_identifier(i);
        joint_angle->set_value(45.0);
    }

    // Only print third joint
    for (auto joint_angle : constrained_joint_angles.joint_angles().joint_angles())
    {
        if (joint_angle.joint_identifier() == 3)
        {
            std::cout << "joint 3 = " << joint_angle.value() << std::endl;
            break;
        }
    }

    // Print all received joint_angle
    for (auto joint_angle : constrained_joint_angles.joint_angles().joint_angles())
    {
        std::cout << "[" << joint_angle.value() << "] = " << joint_angle.joint_identifier() << std::endl; 
    }
}


void example_manipulation_protobuf_helpers()
{
    // Google Protocol Buffer offers some helpers which are available through many .h files.
    // To see the complete list please visit the C++ reference guide on this website:
    // https://developers.google.com/protocol-buffers/docs/reference/cpp/
 
    // In the next example we'll show how to serialize and deserialize to a JSON string.
    // JSON string manipulation is offer by the include file google/protobuf/util/json_util.h
 
    // Creating a message to work with
    auto action_to_serialize = k_api::Base::Action();
    action_to_serialize.set_name("Serialized/Deserialized action");
    
    // Now serializing to JSON string
    std::string serialized_json;
    google::protobuf::util::MessageToJsonString(action_to_serialize, &serialized_json);
    std::cout << "JSON String : " << std::endl;
    std::cout << serialized_json << std::endl;
    /* Expected output:
    JSON String : 
    {"name":"Serialized/Deserialized action"}
    */

    // Now deserialize a JSON string to a message
    auto deserializedAction = k_api::Base::Action();
    google::protobuf::util::JsonStringToMessage(serialized_json, &deserializedAction);
    std::cout << "Google Protocol Buffer message : " << std::endl;
    std::cout << deserializedAction.name() << std::endl; // unfortunately, messages are not easily printable without first having been serialized
    /* Expected output:
    Google Protocol Buffer message : 
    Serialized/Deserialized action
    */
}


int main(int argc, char **argv)
{
    // Basic manipulation
    example_manipulation_protobuf_basic();
    
    // Manipulating messages with nested messages
    example_manipulation_protobuf_object();

    // Manipulation messages containing lists
    example_manipulation_protobuf_list();

    // Using the google::protobuf helper functions
    example_manipulation_protobuf_helpers();
}
